package edu.wpi.first.wpilibj.defaultCode;

import hotbots.HotPID;
import edu.wpi.first.wpilibj.*;
import java.util.TimerTask;

/**
 *
 * @author Wesley
 */
public class DriveTrain extends TimerTask
{
    private DriveQuadEncoder flDriveMtrEnc;
    private DriveQuadEncoder frDriveMtrEnc;
    private DriveQuadEncoder rlDriveMtrEnc;
    private DriveQuadEncoder rrDriveMtrEnc;

    private SteerAngCounter flRrSteerCnt;
    private SteerAngCounter frRlSteerCnt;

    private Jaguar flDriveMtr;
    private Jaguar frDriveMtr;
    private Jaguar rlDriveMtr;
    private Jaguar rrDriveMtr;

    private Jaguar flRrSteerMtr;
    private Jaguar frRlSteerMtr;

    private Joystick driverJoy;

///////////////////////////////////////////////////
// only use one or the other set of steering PID's
    private boolean useHotPID = false;

    private PIDController frRlSteerPID;
    private PIDController flRrSteerPID;

    private HotPID frRlSteerHotPID;
    private HotPID flRrSteerHotPID;
////////////////////////////////////////////////////

    private PIDController flMtrPID;
    private PIDController frMtrPID;
    private PIDController rlMtrPID;
    private PIDController rrMtrPID;

    private WheelState wheelStates;


    public DriveTrain(Joystick driverJoy)
    {
        System.out.println("DriveTrain::DriveTrain");

        this.driverJoy = driverJoy;
        wheelStates = new WheelState();

        /**
         * Initialize the PID controllers for the wheel speed and rotation
         */
        if ( useHotPID )
        {
            System.out.println("DriveTrain::Create Steer HotPID");
            createSteerHotPIDs();
        }
        else
        {
            System.out.println("DriveTrain::Create Steer pid");
            createSteerPIDs();
        }

        createMotorPIDs();
    }

    /**
     * Given the input values, calculate how each of the wheels is supposed to
     * move and set the PID controllers appropriately
     * @param xVal x-value of the joystick vector in degrees
     * @param yVal y-value of the joystick vector in degrees
     * @param twist twist magnitude
     */
    public void driveTo(double xVal, double yVal, double twist)
    {
        int xSign = Utils.signOf(xVal);
        int ySign = -1*Utils.signOf(yVal);
        xVal = xSign*xVal*xVal;
        yVal = ySign*yVal*yVal;

        // square for joystick sensitivity
        double vMag = Math.sqrt(xVal*xVal + yVal*yVal) * ySign;
        double angle = Utils.mod360(Utils.calculateAng(xVal, yVal));
        
        angle = Utils.mod360( angle - 90 );


        // create dead zone in center of joystick
        if ( Math.abs(vMag) <= 0.1 ){
            vMag = 0;
            angle = 0;
        }
        //angle = 45; // fixed value for testing

        // Scale the magnitude if it is more than 1
        vMag = Utils.limitValue( vMag, 1.0 );

//        SmartDashboard.log(angle, "FR Angle");
//        SmartDashboard.log(Utils.mod360(-angle), "FL Angle");
//        SmartDashboard.log(frRlSteerPID.onTarget(), "FRRL On Target");
//        SmartDashboard.log(flRrSteerPID.onTarget(), "FLRR On Target");


       // do not deal with twist now
       twist = 0;
       if(Math.abs(twist) <= .1)
       {
            // Normal Steering
            setWheelStates();
            if ( useHotPID )
            {
                  frRlSteerHotPID.setSPLimits(angle,angle);
                  flRrSteerHotPID.setSPLimits(angle,angle);
            }
            else
            {
                  frRlSteerPID.setSetpoint(angle);
                  flRrSteerPID.setSetpoint(angle);
            }

            flMtrPID.setSetpoint(vMag);
            frMtrPID.setSetpoint(vMag);
            rlMtrPID.setSetpoint(vMag);
            rrMtrPID.setSetpoint(vMag);
        }
        else
        {
            twistWheels();
        }
    }
    
    private void twistWheels()
    {
        //Move wheels to turn position  /     \
        //                              \     /
        //                              -     +
        //                              -     + 
    }

    private void setWheelStates()
    {
       /**
 *     TODO: Revisit the shortest rotary route. Temporarily removed for debugging
 *           double checkAngle = getRotaryShortestPath(currAngle, angle);
 *
 *            //If there is a shorter path for the wheels, swap the sign on the
 *           //motors
 *            if(checkAngle != angle)
 *            {
 *                wheelStates.FL_SIGN *= -1;
 *                wheelStates.FR_SIGN *= -1;
 *                wheelStates.RL_SIGN *= -1;
 *                wheelStates.RR_SIGN *= -1;
 *           }
 */
    }

    /**
     * Sets the appropriate PID controller values to drive the robot as fast as
     * possible to the given angle
     * @param angle Angle to drive to
     */
    public void driveTo(double angle)
    {
        System.out.println("DriveTrain::driveTo(angle)");
    }

    /**
     * RotaryShortestPath()
     *
     * Returns the shortest delta path from current angle to target angle.
     *
     */
    private double getRotaryShortestPath(double target, double current )
    {
        System.out.println("DriveTrain::getRotaryShortestPath");
        double output = target - current;

        output = output % 360.0;

        if( output < -180.0 )
            output = 360.0 + output;
        if( output > 180.0 )
            output = output - 360.0;

        return output;
    }

    /**
     * Realign wheels to zero
     */
    public void zeroWheels()
    {

    }
       
    private PIDController createMotorPID( PIDSource source, PIDOutput output )
    {
        PIDController ctlr = createPID(Constants.P_DRIVE_CONST, Constants.I_DRIVE_CONST,
                Constants.D_DRIVE_CONST, source, output );
        ctlr.setTolerance(Constants.PID_TOLERANCE);
        ctlr.setInputRange(-1, 1);
        ctlr.setContinuous();
        ctlr.enable();
        return ctlr;
    }

    private PIDController createSteerPID( PIDSource source, PIDOutput output )
    {
       PIDController ctlr = createPID(Constants.P_STEER_CONST, Constants.I_STEER_CONST, Constants.D_STEER_CONST, source, output );
       ctlr.setTolerance(Constants.STEER_PID_TOLERANCE);
       ctlr.setInputRange(0, 360);
     
       ctlr.setContinuous();
       ctlr.enable();
       return ctlr;
    }

    private PIDController createPID( double kP, double kI, double kD, PIDSource source, PIDOutput output )
    {
        return new PIDController( kP, kI, kD, source, output );
    }


    private void createSteerPIDs()
    {
        createSteerMotorEncoder();
        frRlSteerPID = createSteerPID( frRlSteerCnt, frRlSteerMtr );
        flRrSteerPID = createSteerPID( flRrSteerCnt, flRrSteerMtr );
    }

    private void createSteerMotorEncoder()
    {
        flRrSteerCnt = new SteerAngCounter(6, 5, "FL-RR");
        frRlSteerCnt = new SteerAngCounter(12, 11, "FR-RL");

        flRrSteerMtr = new Jaguar(Constants.MODULE_NUM, Constants.FL_RR_STEER_MTR );
        frRlSteerMtr = new Jaguar(Constants.MODULE_NUM, Constants.FR_RL_STEER_MTR );
    }

    private void createSteerHotPIDs()
    {
        createSteerMotorEncoder();
        frRlSteerHotPID = createSteerHotPID( frRlSteerCnt, frRlSteerMtr );
        flRrSteerHotPID = createSteerHotPID( flRrSteerCnt, flRrSteerMtr );
    }

    private void createMotorPIDs()
    {
        flDriveMtrEnc = new DriveQuadEncoder(1, 2, "FL");
        frDriveMtrEnc = new DriveQuadEncoder(7, 8, "FR");
        rlDriveMtrEnc = new DriveQuadEncoder(3, 4, "RL");
        rrDriveMtrEnc = new DriveQuadEncoder(9,10, "RR");

        flDriveMtr = new Jaguar(Constants.MODULE_NUM, Constants.FL_DRIVE_MOTOR);
        frDriveMtr = new Jaguar(Constants.MODULE_NUM, Constants.FR_DRIVE_MOTOR);
        rlDriveMtr = new Jaguar(Constants.MODULE_NUM, Constants.RL_DRIVE_MOTOR);
        rrDriveMtr = new Jaguar(Constants.MODULE_NUM, Constants.RR_DRIVE_MOTOR);

        flMtrPID = createMotorPID( flDriveMtrEnc, flDriveMtr );
        frMtrPID = createMotorPID( frDriveMtrEnc, frDriveMtr );
        rlMtrPID = createMotorPID( rlDriveMtrEnc, rlDriveMtr );
        rrMtrPID = createMotorPID( rrDriveMtrEnc, rrDriveMtr );
    }

    private HotPID createSteerHotPID( PIDSource source, PIDOutput output )
    {
        HotPID ctlr = new HotPID( Constants.P_STEER_CONST, Constants.I_STEER_CONST, Constants.D_STEER_CONST, Constants.STEER_PID_TOLERANCE);
        ctlr.setTolerance(Constants.STEER_PID_TOLERANCE);
        ctlr.setPVLimits( 0, 360 );
        ctlr.enable();
        return ctlr;
    }

    public void run() {
        double xVal = driverJoy.getX();
        double yVal = driverJoy.getY();

        driveTo(xVal, yVal, 0);
    }
}
